#include "lidar_localization/models/graph_optimizer/g2o/edge/edge_match.hpp"
#include "lidar_localization/models/graph_optimizer/preintegration/preintegration.hpp"
namespace g2o
{
    EdgeMatch::EdgeMatch() : BaseMultiEdge<6, std::pair<Eigen::Vector3d, Eigen::Quaterniond>>()
    {
        resize(2);
    }
    void EdgeMatch::computeError()
    {
        Eigen::Vector3d Pi = (dynamic_cast<VertexVec *>(_vertices[0]))->estimate();
        Eigen::Quaterniond Qi = (dynamic_cast<VertexQ *>(_vertices[1]))->estimate();

        _error.head(3) = Pi - _measurement.first;
        _error.tail(3) = 2 * (_measurement.second.inverse() * Qi).vec();
    }
    void EdgeMatch::linearizeOplus()
    {
        Eigen::Vector3d Pi = (dynamic_cast<VertexVec *>(_vertices[0]))->estimate();
        Eigen::Quaterniond Qi = (dynamic_cast<VertexQ *>(_vertices[1]))->estimate();

        Eigen::Matrix<double, 6, 6> jacs;
        jacs.setZero();
        jacs.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
        jacs.block<3, 3>(3, 3) = (lidar_localization::Qleft(_measurement.second.inverse() * Qi) ).bottomRightCorner<3, 3>();

        _jacobianOplus[0] = jacs.block<6, 3>(0, 0);
        _jacobianOplus[1] = jacs.block<6, 3>(0, 3);
    }
} // namespace g2o